using BepuPhysics.CollisionDetection.CollisionTasks;
using BepuPhysics.Trees;
using BepuUtilities;
using BepuUtilities.Memory;
using System;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics.Collidables
{
    public unsafe struct ShapeTreeOverlapEnumerator<TSubpairOverlaps> : IBreakableForEach<int> where TSubpairOverlaps : ICollisionTaskSubpairOverlaps
    {
        public BufferPool Pool;
        public void* Overlaps;
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool LoopBody(int i)
        {
            Unsafe.AsRef<TSubpairOverlaps>(Overlaps).Allocate(Pool) = i;
            return true;
        }
    }
    public unsafe struct ShapeTreeSweepLeafTester<TOverlaps> : ISweepLeafTester where TOverlaps : ICollisionTaskSubpairOverlaps
    {
        public BufferPool Pool;
        public void* Overlaps;
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void TestLeaf(int leafIndex, ref float maximumT)
        {
            Unsafe.AsRef<TOverlaps>(Overlaps).Allocate(Pool) = leafIndex;
        }
    }

    /// <summary>
    /// 设计成包含一整串三角形的形状。三角形碰撞和光线测试是单边的;只有在右手坐标中顺时针缠绕三角形或在左手坐标中逆时针缠绕的测试才会生成接触。
    /// </summary>
    public struct Mesh : IHomogeneousCompoundShape<Triangle, TriangleWide>
    {
        /// <summary>
        /// 网格的加速结构。
        /// </summary>
        public Tree Tree;
        /// <summary>
        /// 组成网格的三角形的缓冲区。三角形只会与在右手坐标中顺时针缠绕或在左手坐标中逆时针缠绕的测试发生冲突。
        /// </summary>
        public Buffer<Triangle> Triangles;
        internal Vector3 scale;
        internal Vector3 inverseScale;
        /// <summary>
        /// 获取或设置网格的比例。
        /// </summary>
        public Vector3 Scale
        {
            get
            {
                return scale;
            }
            set
            {
                scale = value;
                inverseScale = new Vector3(
                    value.X != 0 ? 1f / value.X : float.MaxValue,
                    value.Y != 0 ? 1f / value.Y : float.MaxValue,
                    value.Z != 0 ? 1f / value.Z : float.MaxValue);
            }
        }

        /// <summary>
        /// 创建网格形状。
        /// </summary>
        /// <param name="triangles">要在网格中使用的三角形。</param>
        /// <param name="scale">缩放以在运行时应用于所有顶点。
        /// 请注意,比例不会烘焙到三角形或加速结构中;同一组三角形和加速结构可以用于具有不同比例的多个网格实例。</param>
        /// <param name="pool">用于分配加速结构的池。</param>
        public Mesh(Buffer<Triangle> triangles, in Vector3 scale, BufferPool pool) : this()
        {
            Triangles = triangles;
            Tree = new Tree(pool, triangles.Length);
            pool.Take<BoundingBox>(triangles.Length, out var boundingBoxes);
            for (int i = 0; i < triangles.Length; ++i)
            {
                ref var t = ref triangles[i];
                ref var bounds = ref boundingBoxes[i];
                bounds.Min = Vector3.Min(t.A, Vector3.Min(t.B, t.C));
                bounds.Max = Vector3.Max(t.A, Vector3.Max(t.B, t.C));
            }
            Tree.SweepBuild(pool, boundingBoxes);
            pool.Return(ref boundingBoxes);
            Scale = scale;
        }

        /// <summary>
        /// 从先前由序列化函数存储的字节缓冲区中存储的数据加载网格。
        /// </summary>
        /// <param name="data">要从中加载网格的数据。</param>
        /// <param name="pool">用于创建网格的池。</param>
        public unsafe Mesh(Span<byte> data, BufferPool pool)
        {
            if (data.Length < 16)
                throw new ArgumentException("Data is not large enough to contain a header.");
            this = default;
            Scale = Unsafe.As<byte, Vector3>(ref data[0]);
            var triangleCount = Unsafe.As<byte, int>(ref data[12]);
            var triangleByteCount = triangleCount * sizeof(Triangle);
            if (data.Length < 4 + triangleCount * sizeof(Triangle))
                throw new ArgumentException($"Data is not large enough to contain the number of triangles specified in the header, {triangleCount}.");
            Tree = new Tree(data.Slice(16 + triangleByteCount, data.Length - 16 - triangleByteCount), pool);
            pool.Take(triangleCount, out Triangles);
            Unsafe.CopyBlockUnaligned(ref *(byte*)Triangles.Memory, ref data[16], (uint)triangleByteCount);
        }

        /// <summary>
        /// 获取在字节缓冲区中存储给定网格所需的字节数。
        /// </summary>
        /// <param name="Mesh">要测量的网格。</param>
        /// <return>存储网格所需的字节数。</return>
        public unsafe int GetSerializedByteCount()
        {
            return 16 + Triangles.Length * sizeof(Triangle) + Tree.GetSerializedByteCount();
        }

        /// <summary>
        /// 将网格数据写入字节缓冲区。
        /// </summary>
        /// <param name="Mesh">要写入字节缓冲区的网格。</param>
        /// <param name="data">存储网格的字节缓冲区。</param>
        public unsafe void Serialize(Span<byte> data)
        {
            var requiredSizeInBytes = GetSerializedByteCount();
            if (data.Length < requiredSizeInBytes)
                throw new ArgumentException($"Target span size {data.Length} is less than the required size of {requiredSizeInBytes}.");
            Unsafe.As<byte, Vector3>(ref data[0]) = scale;
            Unsafe.As<byte, int>(ref data[12]) = Triangles.Length;
            var triangleByteCount = Triangles.Length * sizeof(Triangle);
            Unsafe.CopyBlockUnaligned(ref data[16], ref *(byte*)Triangles.Memory, (uint)triangleByteCount);
            Tree.Serialize(data.Slice(16 + triangleByteCount, data.Length - 16 - triangleByteCount));
        }

        public int ChildCount => Triangles.Length;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe void GetLocalChild(int triangleIndex, out Triangle target)
        {
            ref var source = ref Triangles[triangleIndex];
            target.A = scale * source.A;
            target.B = scale * source.B;
            target.C = scale * source.C;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe void GetPosedLocalChild(int triangleIndex, out Triangle target, out RigidPose childPose)
        {
            GetLocalChild(triangleIndex, out target);
            childPose = new RigidPose((target.A + target.B + target.C) * (1f / 3f));
            target.A -= childPose.Position;
            target.B -= childPose.Position;
            target.C -= childPose.Position;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe void GetLocalChild(int triangleIndex, ref TriangleWide target)
        {
            // 这会在给定宽实例的第一个插槽中插入一个三角形。
            ref var source = ref Triangles[triangleIndex];
            Vector3Wide.WriteFirst(source.A * scale, ref target.A);
            Vector3Wide.WriteFirst(source.B * scale, ref target.B);
            Vector3Wide.WriteFirst(source.C * scale, ref target.C);
        }

        public void ComputeBounds(in Quaternion orientation, out Vector3 min, out Vector3 max)
        {
            Matrix3x3.CreateFromQuaternion(orientation, out var r);
            min = new Vector3(float.MaxValue);
            max = new Vector3(-float.MaxValue);
            for (int i = 0; i < Triangles.Length; ++i)
            {
                // 这不是网格的理想边界框计算。
                // -你也许能够从广泛的矢量化中获得胜利。
                // -索引平滑网格的最大/最小运算量往往是其三分之一。
                // -更好的是一组已知可以完全包围网格的极值点,从而消除了测试绝大多数网格的需要。
                // 但是,只有在动态网格很常见的情况下,优化这一点才有意义,而它们真的、真的、真的不应该是这样的。
                ref var triangle = ref Triangles[i];
                Matrix3x3.Transform(scale * triangle.A, r, out var a);
                Matrix3x3.Transform(scale * triangle.B, r, out var b);
                Matrix3x3.Transform(scale * triangle.C, r, out var c);
                var min0 = Vector3.Min(a, b);
                var min1 = Vector3.Min(c, min);
                var max0 = Vector3.Max(a, b);
                var max1 = Vector3.Max(c, max);
                min = Vector3.Min(min0, min1);
                max = Vector3.Max(max0, max1);
            }
        }

        public ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches)
        {
            return new HomogeneousCompoundShapeBatch<Mesh, Triangle, TriangleWide>(pool, initialCapacity);
        }

        unsafe struct HitLeafTester<T> : IRayLeafTester where T : IShapeRayHitHandler
        {
            public Triangle* Triangles;
            public T HitHandler;
            public Matrix3x3 Orientation;
            public Vector3 InverseScale;
            public RayData OriginalRay;

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public unsafe void TestLeaf(int leafIndex, RayData* rayData, float* maximumT)
            {
                ref var triangle = ref Triangles[leafIndex];
                if (Triangle.RayTest(triangle.A, triangle.B, triangle.C, rayData->Origin, rayData->Direction, out var t, out var normal) && t <= *maximumT)
                {
                    // 在将击球传给用户之前,将击球拉回世界空间。这样做的成本确实稍高一些,但不是很多,而且您可以随时在以后添加专门的非转换路径。
                    Matrix3x3.Transform(normal * InverseScale, Orientation, out normal);
                    normal = Vector3.Normalize(normal);
                    HitHandler.OnRayHit(OriginalRay, ref *maximumT, t, normal, leafIndex);
                }
            }
        }

        /// <summary>
        /// 将光线投射到网格上。对每个考生和每次命中执行回调。
        /// </summary>
        /// <typeparam name="TRayHitHandler">要为每个候选测试执行并命中的回调类型。</typeparam>
        /// <param name="pose">光线测试期间网格的姿势。</param>
        /// <param name="ray">要对网格进行测试的射线。</param>
        /// <param name="maxumT">射线的最大长度,以射线方向长度为单位。</param>
        /// <param name="hitHandler">对每次命中执行的回调。</param>
        public unsafe void RayTest<TRayHitHandler>(in RigidPose pose, in RayData ray, ref float maximumT, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler
        {
            HitLeafTester<TRayHitHandler> leafTester;
            leafTester.Triangles = Triangles.Memory;
            leafTester.HitHandler = hitHandler;
            Matrix3x3.CreateFromQuaternion(pose.Orientation, out leafTester.Orientation);
            leafTester.InverseScale = inverseScale;
            leafTester.OriginalRay = ray;
            Matrix3x3.TransformTranspose(ray.Origin - pose.Position, leafTester.Orientation, out var localOrigin);
            Matrix3x3.TransformTranspose(ray.Direction, leafTester.Orientation, out var localDirection);
            localOrigin *= inverseScale;
            localDirection *= inverseScale;
            Tree.RayCast(localOrigin, localDirection, ref maximumT, ref leafTester);
            // 树叶测试仪可能变异了命中处理器;把它复制回来。
            hitHandler = leafTester.HitHandler;
        }

        /// <summary>
        /// 同时将一束光线投射到网格上,为每个测试候选对象和每次命中执行回调。
        /// </summary>
        /// <typeparam name="TRayHitHandler">要为每个射线测试候选对象和每个命中执行的回调类型。</typeparam>
        /// <param name="pose">光线测试期间网格的姿势。</param>
        /// <param name="ray">要投射到网格上的光线集。</param>
        /// <param name="hitHandler">要执行的回调。</param>
        public unsafe void RayTest<TRayHitHandler>(in RigidPose pose, ref RaySource rays, ref TRayHitHandler hitHandler) where TRayHitHandler : struct, IShapeRayHitHandler
        {
            HitLeafTester<TRayHitHandler> leafTester;
            leafTester.Triangles = Triangles.Memory;
            leafTester.HitHandler = hitHandler;
            Matrix3x3.CreateFromQuaternion(pose.Orientation, out leafTester.Orientation);
            Matrix3x3.Transpose(leafTester.Orientation, out var inverseOrientation);
            leafTester.InverseScale = inverseScale;
            for (int i = 0; i < rays.RayCount; ++i)
            {
                rays.GetRay(i, out var ray, out var maximumT);
                leafTester.OriginalRay = *ray;
                Matrix3x3.Transform(ray->Origin - pose.Position, inverseOrientation, out var localOrigin);
                Matrix3x3.Transform(ray->Direction, inverseOrientation, out var localDirection);
                localOrigin *= inverseScale;
                localDirection *= inverseScale;
                Tree.RayCast(localOrigin, localDirection, ref *maximumT, ref leafTester);
            }
            // 树叶测试仪可能变异了命中处理器;把它复制回来。
            hitHandler = leafTester.HitHandler;
        }

        public unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(ref Buffer<OverlapQueryForPair> pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
            where TOverlaps : struct, ICollisionTaskOverlaps<TSubpairOverlaps>
            where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps
        {
            // 目前,我们没有使用任何棘手的东西。只需按顺序遍历每个孩子在树上的位置即可。
            // TODO：这将对大量缓存未命中进行序列化。您可能会从"同时"遍历所有对中获得一些好处-也就是说,
            // 利用我们有很多独立查询的事实来确保CPU总是有事情要做。
            ShapeTreeOverlapEnumerator<TSubpairOverlaps> enumerator;
            enumerator.Pool = pool;
            for (int i = 0; i < pairs.Length; ++i)
            {
                ref var pair = ref pairs[i];
                ref var mesh = ref Unsafe.AsRef<Mesh>(pair.Container);
                var scaledMin = mesh.inverseScale * pair.Min;
                var scaledMax = mesh.inverseScale * pair.Max;
                enumerator.Overlaps = Unsafe.AsPointer(ref overlaps.GetOverlapsForPair(i));
                // 取最小/最大值来补偿负值刻度。
                mesh.Tree.GetOverlaps(Vector3.Min(scaledMin, scaledMax), Vector3.Max(scaledMin, scaledMax), ref enumerator);
            }
        }

        public unsafe void FindLocalOverlaps<TOverlaps>(in Vector3 min, in Vector3 max, in Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps)
            where TOverlaps : ICollisionTaskSubpairOverlaps
        {
            var scaledMin = min * inverseScale;
            var scaledMax = max * inverseScale;
            var scaledSweep = sweep * inverseScale;
            ShapeTreeSweepLeafTester<TOverlaps> enumerator;
            enumerator.Pool = pool;
            enumerator.Overlaps = overlaps;
            // 取最小/最大值来补偿负值刻度。
            Tree.Sweep(Vector3.Min(scaledMin, scaledMax), Vector3.Max(scaledMin, scaledMax), scaledSweep, maximumT, ref enumerator);
        }

        public struct MeshTriangleSource : ITriangleSource
        {
            Mesh mesh;
            int triangleIndex;

            public MeshTriangleSource(in Mesh mesh)
            {
                this.mesh = mesh;
                triangleIndex = 0;
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public bool GetNextTriangle(out Vector3 a, out Vector3 b, out Vector3 c)
            {
                if (triangleIndex < mesh.Triangles.Length)
                {
                    ref var triangle = ref mesh.Triangles[triangleIndex++];
                    a = triangle.A * mesh.scale;
                    b = triangle.B * mesh.scale;
                    c = triangle.C * mesh.scale;
                    return true;
                }
                a = default;
                b = default;
                c = default;
                return false;
            }
        }

        /// <summary>
        /// 从网格外壳线中的所有点减去新中心。
        /// </summary>
        /// <param name="newCenter">所有点将相对于的新中心。</param>
        public unsafe void Recenter(in Vector3 newCenter)
        {
            var scaledOffset = newCenter * inverseScale;
            for (int i = 0; i < Triangles.Length; ++i)
            {
                ref var triangle = ref Triangles[i];
                triangle.A -= scaledOffset;
                triangle.B -= scaledOffset;
                triangle.C -= scaledOffset;
            }
            for (int i = 0; i < Tree.NodeCount; ++i)
            {
                ref var node = ref Tree.Nodes[i];
                node.A.Min -= scaledOffset;
                node.A.Max -= scaledOffset;
                node.B.Min -= scaledOffset;
                node.B.Max -= scaledOffset;
            }
        }

        /// <summary>
        /// 计算网格围绕其体积中心的惯性,并将网格点围绕其重新居中。
        /// 假定网格是闭合的,应将其视为实体。
        /// </summary>
        /// <param name="quality">用于缩放惯性张量的质量。</param>
        /// <param name="惯性">闭合网格的惯性张量。</param>
        /// <param name="center">闭合网格的中心。</param>
        public void ComputeClosedInertia(float mass, out BodyInertia inertia, out Vector3 center)
        {
            var triangleSource = new MeshTriangleSource(this);
            MeshInertiaHelper.ComputeClosedInertia(ref triangleSource, mass, out _, out var inertiaTensor, out center);
            MeshInertiaHelper.GetInertiaOffset(mass, center, out var inertiaOffset);
            Symmetric3x3.Add(inertiaTensor, inertiaOffset, out var recenteredInertia);
            Recenter(center);
            Symmetric3x3.Invert(recenteredInertia, out inertia.InverseInertiaTensor);
            inertia.InverseMass = 1f / mass;
        }

        /// <summary>
        /// 计算网格的惯性。
        /// 假定网格是闭合的,应将其视为实体。
        /// </summary>
        /// <param name="quality">用于缩放惯性张量的质量。</param>
        /// <param name="惯性">闭合网格的惯性。</param>
        public void ComputeClosedInertia(float mass, out BodyInertia inertia)
        {
            var triangleSource = new MeshTriangleSource(this);
            MeshInertiaHelper.ComputeClosedInertia(ref triangleSource, mass, out _, out var inertiaTensor);
            inertia.InverseMass = 1f / mass;
            Symmetric3x3.Invert(inertiaTensor, out inertia.InverseInertiaTensor);
        }

        /// <summary>
        /// 计算网格的体积和质心。假定网格是闭合的,应将其视为实体。
        /// </summary>
        /// <param name="volume">闭合网格的体积。</param>
        /// <param name="center">闭合网格的质心。</param>
        public void ComputeClosedCenterOfMass(out float volume, out Vector3 center)
        {
            var triangleSource = new MeshTriangleSource(this);
            MeshInertiaHelper.ComputeClosedCenterOfMass(ref triangleSource, out volume, out center);
        }

        /// <summary>
        /// 计算网格的质心。
        /// 假定网格是闭合的,应将其视为实体。
        /// </summary>
        /// <return>闭合网格的质心。</return>
        public Vector3 ComputeClosedCenterOfMass()
        {
            var triangleSource = new MeshTriangleSource(this);
            MeshInertiaHelper.ComputeClosedCenterOfMass(ref triangleSource, out _, out var center);
            return center;
        }

        /// <summary>
        /// 计算网格围绕其体积中心的惯性,并将网格点围绕其重新居中。
        /// 假定网格是开放的,应将其视为三角形汤。
        /// </summary>
        /// <param name="quality">用于缩放惯性张量的质量。</param>
        /// <param name="惯性">闭合网格的惯性张量。</param>
        /// <param name="center">开放网格的中心。</param>
        public void ComputeOpenInertia(float mass, out BodyInertia inertia, out Vector3 center)
        {
            var triangleSource = new MeshTriangleSource(this);
            MeshInertiaHelper.ComputeOpenInertia(ref triangleSource, mass, out var inertiaTensor, out center);
            MeshInertiaHelper.GetInertiaOffset(mass, center, out var inertiaOffset);
            Symmetric3x3.Add(inertiaTensor, inertiaOffset, out var recenteredInertia);
            Recenter(center);
            Symmetric3x3.Invert(recenteredInertia, out inertia.InverseInertiaTensor);
            inertia.InverseMass = 1f / mass;
        }

        /// <summary>
        /// 计算网格的惯性。
        /// 假定网格是开放的,应将其视为三角形汤。
        /// </summary>
        /// <param name="quality">用于缩放惯性张量的质量。</param>
        /// <param name="惯性">开放网格的惯性。</param>
        public void ComputeOpenInertia(float mass, out BodyInertia inertia)
        {
            var triangleSource = new MeshTriangleSource(this);
            MeshInertiaHelper.ComputeOpenInertia(ref triangleSource, mass, out var inertiaTensor);
            inertia.InverseMass = 1f / mass;
            Symmetric3x3.Invert(inertiaTensor, out inertia.InverseInertiaTensor);
        }

        /// <summary>
        /// 计算网格的质心。
        /// 假定网格是开放的,应将其视为三角形汤。
        /// </summary>
        /// <return>开放网格的质心。</return>
        public Vector3 ComputeOpenCenterOfMass()
        {
            var triangleSource = new MeshTriangleSource(this);
            return MeshInertiaHelper.ComputeOpenCenterOfMass(ref triangleSource);
        }

        /// <summary>
        /// 将网格的资源返回到缓冲池。
        /// </summary>
        /// <param name="BufferPool">要将网格资源返回到的池。</param>
        public void Dispose(BufferPool bufferPool)
        {
            bufferPool.Return(ref Triangles);
            Tree.Dispose(bufferPool);
        }


        /// <summary>
        /// 网格形状的类型ID。
        /// </summary>
        public const int Id = 8;
        public int TypeId => Id;

    }
}
